/*
 *  lib-smv: Matrix - Vector Library targeted to Structural Mechanics Problems
 *  Copyright (C) 2006-2008  St.Brcic, Lj.Zugic
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
 
/** @file Crout.cpp
 *  @brief Implementation of class Crout - LU decomposition of general matrices
 *
 *  Crout's LU decomposition     A = L * U 
 *
 *  DIRECT SOLUTION OF LINEAR EQUATIONS A * x = b or A * X = B
 *  
 *  LU decomposition of general (non-symmetric) matrices used for:
 *  (1) Solution of linear equations
 *  (2) Inverse of a general matrix
 *  (3) Determinant of a general matrix
 * 
 */ 

#include "Crout.h"
namespace smv_lib
{

//! empty (default) constructor
Crout::Crout(void)
{
  isMatrix_A    = false;
  isNameGiven   = false;
  isVector_b    = false;
  isMatrix_B    = false;
  isDecomposed  = false;
  isInverse     = false;
  isSolved      = false;
  isMore        = false;
}

//! constructor with a given matrix (to obtain LU decomposition only)
Crout::Crout(const Matrix& AA) : A(AA)
{
  N = A.getRows();
  assert(N == A.getCols());
  isMatrix_A  = true;
  Name = A.getName();
  if (Name == "MATRIX")
    isNameGiven = false;
  else
    isNameGiven = true;
  isVector_b    = false;
  isMatrix_B    = false;
  isDecomposed  = false;
  isInverse     = false;
  isSolved      = false;
  isMore        = false;
}

//! constructor with a given matrix A and vector b - to solve equation A*x=b
Crout::Crout(const Matrix& AA, const Vector& bb) : A(AA), b(bb)
{
  N = A.getRows();
  assert(N == A.getCols());
  isMatrix_A  = true;
  Name = A.getName();
  if (Name == "MATRIX")
    isNameGiven = false;
  else
    isNameGiven = true;
  //--- right-hand side vector b
  assert (N == b.getLen());
  isVector_b = true;
  isMatrix_B = false;
  //--- solution vector x
  Vector* tmp = new Vector(N);
  assert(tmp != NULL);
  x = (*tmp);
  isDecomposed  = false;
  isInverse     = false;
  isSolved      = false;
  isMore        = false;
}

//! constructor with a given matrix A and a matrix B - solution of equations A*X=B
Crout::Crout(const Matrix& AA, const Matrix& BB) : A(AA), B(BB)
{
  //--- matrix A (coefficient matrix)
  N = A.getRows();
  assert(N == A.getCols());
  isMatrix_A = true;
  Name = A.getName();
  if (Name == "MATRIX")
    isNameGiven = false;
  else
    isNameGiven = true;
  //--- matrix B (right-hand side)
  assert (N == B.getRows());
  M = B.getCols();
  isMatrix_B = true;
  isVector_b = false;
  //--- matrix X (solution of equations A * X = B)
  Matrix* tmp = new Matrix(N,M);
  assert(tmp != NULL);
  X = (*tmp);
  isDecomposed  = false;
  isInverse     = false;
  isSolved      = false;
  isMore        = true;
}


Crout::~Crout(void)
{
  if (isInverse == true)
    delete A_inv;
}

//! set the coefficient matrix A
void Crout::setMatrix_A(const Matrix& AA)
{
  //--- matrix A
  A = AA;
  N = A.getRows();
  assert(N == A.getCols());
  isMatrix_A  = true;
  Name = A.getName();
  if (Name == "MATRIX")
    isNameGiven = false;
  else
    isNameGiven = true;
  isVector_b    = false;
  isMatrix_B    = false;
  isDecomposed  = false;
  isInverse     = false;
  isSolved      = false;
  isMore        = false;
}

//! set the right-hand side vector b (Matrix A must be given first)
void Crout::setVector_b(const Vector& bb)
{
  if (isMatrix_A == false) {
    cerr << "\nGive the Matrix A first!";
    return;
  }
  b = bb;
  assert(N == b.getLen());
  isVector_b = true;
  isMatrix_B = false;
  //--- solution vector x
  Vector* tmp = new Vector(N);
  assert(tmp != NULL);
  x = (*tmp);
  //isDecomposed  = false;
  isInverse     = false;
  isSolved      = false;
  isMore        = false;
}

//! set the right-hand side matrix B (Matrix A must be given first)
void Crout::setMatrix_B(const Matrix& BB)
{
  if (isMatrix_A == false) {
    cerr << "\nGive the Matrix A first!";
    return;
  }
  //--- matrix B (right-hand side)
  B = BB;
  assert (N == B.getRows());
  M = B.getCols();
  isMatrix_B = true;
  isVector_b = false;
  //--- matrix X (solution of equations A * X = B)
  Matrix* tmp = new Matrix(N,M);
  assert(tmp != NULL);
  X = (*tmp);
  //isDecomposed  = false;
  isInverse     = false;
  isSolved      = false;
  isMore        = true;
}

//========================================== public working functions
//! Crout's LU decomposition of the matrix
void Crout::Decomposition()
{
  //--- make a copy of the original matrix A
  A_LU = A;
  //--- call LU decomposition
  ludcmp();
  //--- make LU factors
  Matrix* temp = new Matrix(N,N);
  assert(temp != NULL);
  L = *temp;
  L.Identity();
  U = *temp;
  double Aij;
  for (int i=0; i<N; i++) {
    for (int j=0; j<N; j++) {
      Aij = A_LU.getElement(i,j);
      if (j >= i)
        U.setElement(i,j,Aij);
      else if (i > j)
        L.setElement(i,j,Aij);
    } // end for j
  } // end for i
  L.setName("Lower Triangle");
  U.setName("Upper Triangle");
  L.Classify();
  U.Classify();
  isDecomposed = true;
  //--- calculate also determinant of the matrix
  Det = Determinant();
}

//! inverse of the matrix A
void Crout::Inverse()
{
  A_inv = new Matrix(N,N);
  assert(A_inv != NULL);
  //--- check if LU decompossition is done
  if (isDecomposed != true)
    Decomposition();
  else
    *A_inv = croutInv();
  isInverse = true;
}

//! solution of linear equations A*x = b or A*X = B
void Crout::Solution()
{
  if (isDecomposed == false) {
    cerr << "\nDecomposition must be done first!";
    return;
  }
  switch (isMore) {
      case false:
         x = croutEqs(b);    // single system of equations
         break;
      case true:
         X = croutEqs(B);    // more systems of equations
         break;
  }
  isSolved = true;
}

//! get lower triangular factor L: A = L*U
Matrix Crout::get_L()
{
  if (isDecomposed == true)
    return L;
  else {
    cerr << "\nDecomposition MUST be done first!\n";
    return A;
  }
}

//! get upper triangular factor U: A= L*U
Matrix Crout::get_U()
{
  if (isDecomposed == true)
    return U;
  else {
    cerr << "\nDecomposition MUST be done first!\n";
    return A;
  }
}

//! get inverse of the input matrix A
Matrix Crout::getInverse()
{
  if (isInverse == true)
    return (*A_inv);
  else {
    cerr << "\nDecomposition MUST be done first!\n";
    cerr << "\nAfter that, call Inversion!\n";
    return A;
  }
}

//! get determinant of the input matrix A
const double Crout::getDeterminant()
{
  if (isDecomposed == true)
    return Det;
  else {
    cerr << "\nDecomposition MUST be done first!\n";
    return (double)1.0;
  }
}

//! get solution of linear equations A * x = b
Vector Crout::getSolution()
{
  assert(isSolved == true);
  return x;
}

//! get one solutions of the system of linear equations A * X = B
Vector Crout::getSolutionNo(int i)
{
  assert(isSolved == true);
  assert(i >=0 && i<M);
  return (X.getColData(i));
}

//! get all solutions of the system of linear equations A * X = B
Matrix Crout::getAllSolutions()
{
  assert(isSolved == true);
  return X;
}

//====================== PRIVATE Functions
//
//! LU decomposition - Crout's algorithm
void Crout::ludcmp()
{
  int i,imax = 0,j,k;
  double  big, dum, sum, temp;
  Vector vv(N);
  LU_d = 1.0;
  iVector index(N); // temporary vector
  LU_indx = index;
  double a_ij, a_ik, a_kj, a_imaxk, a_jk, a_jj;
  for (i=0; i<N; i++) {
    big = 0.0;
    for (j=0; j<N; j++)  {
      a_ij = A_LU.getElement(i,j);
      if ((temp = fabs(a_ij)) > big)
        big = temp;
    }
    assert (big != 0.0);
    vv[i] = 1.0 / big;
  } // end for i = 0
  for (j=0; j<N; j++) {
    for (i=0; i<j; i++) {
      a_ij = A_LU.getElement(i,j);
      sum=a_ij;
      for (k=0; k<i; k++) {
        a_ik = A_LU.getElement(i,k);
        a_kj = A_LU.getElement(k,j);
        sum -= a_ik * a_kj;
      }
      A_LU.setElement(i,j,sum);
    } // end for (i=0; i<j;
    big = 0.0;
    for (i=j; i<N; i++) {
      a_ij = A_LU.getElement(i,j);
      sum = a_ij;
      for (k=0; k<j; k++) {
        a_ik = A_LU.getElement(i,k);
        a_kj = A_LU.getElement(k,j);
        sum -= a_ik * a_kj;
      }
      A_LU.setElement(i,j, sum);
      if ((dum = vv[i] * fabs(sum)) >= big) {
        big  = dum;
        imax = i;
      }
    } // end for i = j
    if (j != imax) {
      for (k=0; k<N; k++) {
        a_imaxk = A_LU.getElement(imax,k);
        dum = a_imaxk;
        a_imaxk = A_LU.getElement(j,k);
        a_jk = A_LU.getElement(j,k);
        A_LU.setElement(imax,k, a_jk);
        A_LU.setElement(j,k, dum);
      }
      LU_d = -(LU_d);
      vv[imax] = vv[j];
    } // end if (j != imax)
    LU_indx[j] = imax;
    a_jj = A_LU.getElement(j,j);
    if (a_jj == 0.0)
      A_LU.setElement(j,j, LU_TINY);
    if (j != (N-1)) {
      a_jj = A_LU.getElement(j,j);
      dum = 1.0 / a_jj;
      for (i=j+1; i<N; i++) {
        a_ij = A_LU.getElement(i,j);
        a_ij *= dum;
        A_LU.setElement(i,j, a_ij);
      }
    }
  } // end for (j=0; j<N; j++)
}

//! LU backsubstitution - Crout's algorithm
void Crout::lubksb(Vector& bb)
{
  int i,ip,j;
  double sum;
  double a_ij, a_ii;
  for (i=0; i<N; i++) {
    ip   = LU_indx[i];
    sum  = bb[ip];
    bb[ip]= bb[i];
    for (j=0; j<=i-1; j++) {
      a_ij = A_LU.getElement(i,j);
      sum -= a_ij * bb[j];
    }
    bb[i] = sum;
  }
  for (i=N-1; i>=0; i--) {
    sum = bb[i];
    for (j=i+1; j<N; j++) {
      a_ij = A_LU.getElement(i,j);
      sum -= a_ij * bb[j];
    }
    a_ii = A_LU.getElement(i,i);
    bb[i] = sum / a_ii;   // solution
  }
}

//! determinant of the matrix A
const double Crout::Determinant()
{
  double det = 1.0;
  for (int i=0; i<N; i++)
    det *= U.getElement(i,i);
  return det;
}

//! inverse matrix
Matrix Crout::croutInv()
{
  //--- make a copy of LU decomposed original matrix A
  Matrix ans(A_LU);
  Vector* col = new Vector(N);
  assert (col != NULL);
  //--- algorithm from "NR in C", p.45
  for (int j=0; j<N; j++) {
    for (int k=0; k<N; k++)
      col->setData(k, 0.0);
    col->setData(j, 1.0);
    lubksb(*col);
    for (int i=0; i<N; i++)
      ans.setElement(i,j,col->getData(i));
  }
  delete col;
  return (ans);
}

//! solution of linear equations A * x = b
Vector Crout::croutEqs(const Vector& bb)
{
  if (isDecomposed == false)
    Decomposition();
  Vector ans(bb);       // copy of input Vector b
  lubksb(ans);
  return (ans);
}

//! solution of system of linear equations A * X = B
Matrix Crout::croutEqs(const Matrix& BB)
{
  //--- result matrix
  Matrix ans(N,M);
  //--- right-hand side vector no. i
  Vector* bi = new Vector(N);
  assert(bi != NULL);
  //--- solution vector no. i
  Vector* xi = new Vector(N);
  assert(xi != NULL);

  string solName = "Solution No. ";
  string broj;
  char Broj[10];
  //--- now, solve equations M times
  for (int ii=0; ii<M; ii++) {
    //--- obtain solution
    *bi = BB.getColData(ii);
    *xi = croutEqs(*bi);
    sprintf(Broj, "%d", (ii+1));
    broj = Broj;
    solName = solName + broj;
    xi->setName(solName);
    //--- transfer solution
    ans.setColData(ii,*xi);
  }
  // release dynamic memory
  delete xi;
  delete bi;
  // give solution
  return (ans);
}

void Crout::Write()
{
  string FileName;
  if (isNameGiven == false)
    FileName = "Crout.txt";
  else
    FileName = Name + ".txt";
  this->WriteTo(FileName.c_str());
}

void Crout::WriteTo(const char* file)
{
  //--- open the file
  out.open(file,ios::out);
  assert(out != NULL);
  out << "===================== CROUT'S DECOMPOSITION ==========================" << endl << endl;
  out << "(1) INPUT MATRIX DATA: " << endl;
  out << "    - Name is:  " << Name;
  out << "    - Order N = " << N << endl << endl;
  out << A << endl;
  if (isDecomposed == true) {
    out << "(2) CROUT'S LU FACTORS: " << endl;
    out << "Crout's factor L" << endl;
    out << L.getName() << endl;
    out << L << endl;
    out << "Crout's factor U" << endl;
    out << U.getName() << endl;
    out << U << endl;
    //determinant
    out << "Determinant is " << Det << endl << endl;
    out << "(2a) CONTROL of decomposition: " << endl;
    out << "The product of Crout's factors: " << endl;
    out << " A = L * U " << endl;
    out << L*U << endl << endl;
  } // end if (isDecomposed == true)
  if (isInverse == true) {
    out << "(3) INVERSE MATRIX: " << endl << endl;
    out << (*A_inv) << endl;
    out << "Control: A * A_inv = " << endl;
    out << A*(*A_inv) << endl;
  }
  if (isSolved == true && isMore == false) {
    out << "(4) SOLUTION OF LINEAR EQUATIONS: " << endl << endl;
    out << "    Right-hand side = " << endl;
    out << b << endl;
    out << "    The solution is: " << endl;
    out << x << endl;
  }
  if (isSolved == true && isMore == true) {
    out << "(4) SOLUTION OF THE SYSTEM OF LINEAR EQUATIONS: " << endl << endl;
    for (int i=0; i<M; i++) {
      out << "   System number " << (i+1) << endl;
      out << "   Right-hand side = " << endl;
      out << B.getColData(i) << endl;
      out << "   The solution is: " << endl;
      out << X.getColData(i) << endl << endl;
    }
  }
  out.close();
}

//====================== EXTERNAL FUNCTIONS - LU DECOMPOSITION
//
//
//--------------------------------------------------------- LU Decomposition (Crout's algorithm)
/**
** \brief LU decomposition for general matrices - Crout's algorithm
**
**  INPUT:  
**  A(n,n) - general (double) matrix,destroyed after LU decomposition A = L * U
**  indx(n)- int vector recording row permutations 
**           (d is necessary for backsubstitution and matrix inversion)
**  d      - double value (+ or - 1.0) depending if the number of row interchanges is even or odd 
**           (d is necessary for determinant of a matrix)
**  OUTPUT: 
**  A(n,n) = L * U transformation of the input matrix
**  
**  After LU decomposition, determinent, inverse matrix or 
**  solution of algebraic equations may be obtained
**  LU decomposition is according to "Numerical Recipes in C", pp.39-47
**/
//-----------------------------------------------------------------------------------------------
void LUdcmp(Matrix& A, iVector& indx, double& d)
{
  int i,imax = 0,j,k;
  double  big, dum, sum, temp;
  int n = A.getRows();
  Vector vv(n);
  assert(A.getRows() == A.getCols());
  d = 1.0;
  double a_ij, a_ik, a_kj, a_imaxk, a_jk, a_jj;
  for (i=0; i<n; i++) {
    big = 0.0;
    for (j=0; j<n; j++)  {
      a_ij = A.getElement(i,j);
      if ((temp = fabs(a_ij)) > big)
        big = temp;
    }
    assert (big != 0.0);
    vv[i] = 1.0 / big;
  } // end for i = 0
  for (j=0; j<n; j++) {
    for (i=0; i<j; i++) {
      a_ij = A.getElement(i,j);
      sum=a_ij;
      for (k=0; k<i; k++) {
        a_ik = A.getElement(i,k);
        a_kj = A.getElement(k,j);
        sum -= a_ik * a_kj;
      }
      A.setElement(i,j,sum);
    }
    big = 0.0;
    for (i=j; i<n; i++) {
      a_ij = A.getElement(i,j);
      sum = a_ij;
      for (k=0; k<j; k++) {
        a_ik = A.getElement(i,k);
        a_kj = A.getElement(k,j);
        sum -= a_ik * a_kj;
      }
      A.setElement(i,j, sum);
      if ((dum = vv[i] * fabs(sum)) >= big) {
        big  = dum;
        imax = i;
      }
    }
    if (j != imax) {
      for (k=0; k<n; k++) {
        a_imaxk = A.getElement(imax,k);
        dum = a_imaxk;
        a_imaxk = A.getElement(j,k);
        a_jk = A.getElement(j,k);
        A.setElement(imax,k, a_jk);
        A.setElement(j,k, dum);
      }
      d = -(d);
      vv[imax] = vv[j];
    }
    indx[j] = imax;
    a_jj = A.getElement(j,j);
    if (a_jj == 0.0)
      A.setElement(j,j, LU_TINY);
    if (j != (n-1)) {
      a_jj = A.getElement(j,j);
      dum = 1.0 / a_jj;
      for (i=j+1; i<n; i++) {
        a_ij = A.getElement(i,j);
        a_ij *= dum;
        A.setElement(i,j, a_ij);
      }
    }
  }
}

//------------------------------------------------------------------------ LU BackSubstitution
/**
** \brief Backsubstitution after LU decomposition of general or symmetric matrices 
**
**  Note: backsubstitution routine 'lubksb' is used after Crout's or after Cholesky's
**  decomposition. In the case of Cholesky, input vector indx is not used, i.e.
**  it is set to vector (0,1,2,...,n-1)
**
**  INPUT:  
**  A(n,n) - as output after LU decomposition: A = L * U
**  indx(n)- int vector recording row permutations - output after LU decomposition 
**           (d is necessary for backsubstitution and matrix inversion)
**  b(n)   - the right-hand side vector of the system of equations A*x = b
**           (d is necessary for determinant of a matrix)
**  OUTPUT: 
**  b(n)   - the solution of A*x = b is contained in vector b
**  
**  Matrix A and iVector indx are not modifed in lubksb
**  so, lubksb may be used for multiple right-hand sides
**  LU backsubstitution is according to "Numerical Recipes in C", pp.39-47
**/
//---------------------------------------------------------------------------------------------
void LUbksb(Matrix& A, iVector& indx, Vector& b)
{
  int i,ip,j;
  double sum;
  int n = A.getRows();
  MAT_TYPE type = A.get_MatType();
  //--- for Cholesky's decomposition elements of vector 'indx' are sequential
  if (type == TRIDIAG || type == TRI_SYMM || type == SYMMETRIC) {
    for (int ii=0; ii<n; ii++)
      indx.setData(ii, ii);
  }
  //--- for Crout's decopmosition 'indx' is output from ludcmp
  double a_ij, a_ii;
  for (i=0; i<n; i++) {
    ip   = indx[i];
    sum  = b[ip];
    b[ip]= b[i];
    for (j=0; j<=i-1; j++) {
      a_ij = A.getElement(i,j);
      sum -= a_ij * b[j];
    }
    b[i] = sum;
  }
  for (i=n-1; i>=0; i--) {
    sum = b[i];
    for (j=i+1; j<n; j++) {
      a_ij = A.getElement(i,j);
      sum -= a_ij * b[j];
    }
    a_ii = A.getElement(i,i);
    b[i] = sum / a_ii;
  }
}

}   // end of namespace smv_lib
//============================= end of file
